#include<set>
#include<vector>
#include<map>
#include<math.h>
#include<iostream>
#include<fstream>
#include<sstream>
#include<cstdlib>
#include<gsl/gsl_randist.h>

#ifdef USE_GPU
#include<CL/cl.hpp>
#include"../common/clsafe.h"
#endif
#include"hmm_cnv_dimension.h"
#include"../common/main.hpp"
#include"../common/analyzer.hpp"
#include"../common/io.hpp"
#include"../common/utility.hpp"
#include "hmm_cnv.hpp"
//#include "compute_alpha.hpp"

using namespace std;


void HMM_CNV::init(const ptree & pt){
//  verbose_output = true;
  verbose_output = false;
  LOG_SMALL = log(SMALL);
  LOG_LARGE = log(LARGE);
  observed_filelist = pt.get<string>("observed_filelist");
  total_markers = pt.get<int>("total_markers");
  sd_rr_het = new float[5];
  sd_rr_loh = new float[5];
  sd_baf_percent = new float[100]; 
  sd_rr_alpha = new float[100];
  memset(sd_rr_het,0,sizeof(float)*5);
  memset(sd_rr_loh,0,sizeof(float)*5);
  memset(sd_baf_percent,0,sizeof(float)*100);
  memset(sd_rr_alpha,0,sizeof(float)*100);
  int percents_alpha[]={1,33,66,99};
  vector<int> alpha_percent(percents_alpha,percents_alpha+sizeof(percents_alpha)/sizeof(int));
  this->alpha_percents = alpha_percent;
  for(vector<int>::iterator it = alpha_percents.begin();
  it!=alpha_percents.end();it++){
    int percent = *it;
    ostringstream oss;
    oss<<"lrr_sd_cn.stromal."<<percent;
    sd_rr_alpha[percent]=pt.get<float>(oss.str());
  }
  for(int i=0;i<=4;++i){
    ostringstream oss;
    oss<<"lrr_sd_cn.het."<<i;
    sd_rr_het[i]=pt.get<float>(oss.str());
  }
  for(int i=2;i<=2;++i){
    ostringstream oss;
    oss<<"lrr_sd_cn.loh."<<i;
    sd_rr_loh[i]=pt.get<float>(oss.str());
  }
  int percents[]={0,25,33,50};
  for(int i=0;i<4;++i){
    ostringstream oss;
    oss<<"baf_sd_percent."<<percents[i];
    sd_baf_percent[percents[i]]=pt.get<float>(oss.str());
  }
  sd_rr_normal = pt.get<float>("lrr_sd_normal");
  sd_baf_homo_del = pt.get<float>("baf_sd_homo_del");
  sd_alpha=pt.get<float>("alpha_sd");
  normal_cdf = math->normal_cdf(0,.5,sd_baf_homo_del);

  this->math = new MathUtils(123);
  // initialize the observed data
  this->pos_vec = new int[total_markers];
  this->pfb_vec = new float[total_markers];
  //this->alpha_block_vec = new int[total_markers];
  this->rslist = new string[total_markers];
  this->gap_vec = new int[total_markers];
  this->forward_rescalings = new int[total_markers];
  this->backward_rescalings = new int[total_markers];

  output_dir=pt.get<string>("output_dir");
  cerr<<"Will save results in "<<output_dir<<endl;
  pfb_filename=pt.get<string>("pfb_file");

  cerr<<"Loading signal files defined in "<<observed_filelist<<"\n";
  if (!load_observed_files(observed_filelist)){
    cerr<<"Could not load the set of input files\n";
    exit(1);
  }

  //set_global_alpha();
  //global_alpha = 1;
}

void HMM_CNV::allocate_matrices(){
  if (current_sample==0){
    // this if the first in the loop so we need to allocate matrices
    cerr<<"Total States: "<<states<<endl;
    state_het_ratio = new float[states];
    state_total_alpha = new float[states];
    state_tumorcn_vec = new int[states];
    state_het_ident = new bool[states];
    state_alpha_ident = new bool[states];
    state_alpha_prior = new float[states];
    state_mu_rr = new float[states];
    state_sd_rr = new float[states];
    sample_likelihood_log = new float[samples];
    hmm_matrix_size = states * total_markers;
    this->emission_cache_mat = new float[samples*hmm_matrix_size];
    this->forward_mat = new float[samples*hmm_matrix_size];
    this->backward_mat = new float[samples*hmm_matrix_size];
    this->prob_mat = new float[samples*hmm_matrix_size];
    this->prev_prob_mat = new float[hmm_matrix_size];
    trans_matrix_size = states * states;
    this->sample_trans_mat = new float[samples*trans_matrix_size];
    //this->trans_mat = new float[trans_matrix_size];
    cerr<<"Allocated large matrices\n";
  }
  // States created with appropriate means and SDs. 
  for(int state=0;state<states;++state){
    state_mu_rr[state] = state_object_vector[state].mu_rr;
    //cerr<<"state: "<<state<<" has mu_rr "<<state_mu_rr[state]<<endl;
    state_het_ratio[state] = state_object_vector[state].het_ratio;
    state_total_alpha[state] = state_object_vector[state].total_alpha;
    state_tumorcn_vec[state] = state_object_vector[state].tumor_cn;
    state_het_ident[state] = state_object_vector[state].het_ident;
    state_alpha_ident[state] = state_object_vector[state].alpha_ident;
    state_sd_rr[state] = state_object_vector[state].sd_rr;
    float alpha_prior = math->normal_pdf(state_total_alpha[state],global_alpha,sd_alpha);
    if(alpha_prior<SMALL) alpha_prior = SMALL;
    state_alpha_prior[state] = state_object_vector[state].alpha_prior = 
    alpha_prior; 
  }
  for(int i=0;i<states;++i){
    for(int j=0;j<states;++j){
      //sample_trans_mat[current_sample*trans_matrix_size+i*states+j] = i==j?.99:1./(states-1);
      sample_trans_mat[current_sample*trans_matrix_size+i*states+j] = 1./(states);
    }
  }
  cerr<<"Done init\n";
};

HMM_CNV::HMM_CNV(){
}

bool HMM_CNV::load_observed_files(const string & filelistpath){
  ifstream ifslist(filelistpath.data());
  if (!ifslist.is_open()){
    cerr<<"File list is not found.\n";
    return false;
  }
  filelist_vec.clear();
  string line;
  // skip header
  getline(ifslist,line);
  float epsilon=.01;
  while(getline(ifslist,line)){
    istringstream iss(line);
    string file,annot;
    float g_alpha;
    iss>>file>>annot>>g_alpha;
    if (g_alpha==0) g_alpha+=epsilon;
    else if (g_alpha==1) g_alpha-=epsilon;
    filelist_vec.push_back(file);
    sample_annotations.push_back(annot);
    global_alphas.push_back(g_alpha);
  }
  ifslist.close();
  samples = filelist_vec.size();
  if (samples==0) {
    cerr<<"Must have at least one sample\n";
    return false;
  }
  this->rr_vec = new float[samples*total_markers];
  this->baf_vec = new float[samples*total_markers];
  map<int,int> existing_position_map;
  for(int i=0;i<samples;++i){
    ifstream ifsobs(filelist_vec[i].data());
    if (!ifsobs.is_open()){
      cerr<<"File "<<filelist_vec[i]<<" is not found.\n";
      return false;
    }
    int lastpos = 0;
    getline(ifsobs,line); // skip header
    for(int j=0;j<total_markers;++j){
      getline(ifsobs,line);
      istringstream iss(line);
      float lrr;
      iss>>rslist[j]>>chr>>pos_vec[j]>>baf_vec[i*total_markers+j]>>lrr;
      rr_vec[i*total_markers+j] = exp(lrr);
      gap_vec[j] = pos_vec[j]-lastpos;
      lastpos = pos_vec[j];
      // for the first sample, record all observed positions
      if (i==0) {
        //cerr<<"Mapping position "<<pos_vec[j]<<" as index "<<j<<endl;
        existing_position_map[pos_vec[j]] = j;
      }
    }
    ifsobs.close();
    cerr<<"Loaded "<<filelist_vec[i].data()<<endl;
  }
  // OK now parse the PFB file if it exists
  ifstream ifs_pfb(pfb_filename.data());
  for(int j=0;j<total_markers;++j) pfb_vec[j] = .5;
  if (!ifs_pfb.is_open()){
    cerr<<"Warning: cannot locate a population B allele frequency file.  Will assume .5 for all positions\n";
  }else{
    string line;
    getline(ifs_pfb,line); // skip header
    while(getline(ifs_pfb,line)){
      istringstream iss(line);
      string name,chr;
      int pos;
      float pfb;
      iss>>name>>chr>>pos>>pfb;
      if (existing_position_map.find(pos)!=existing_position_map.end()){
        pfb_vec[existing_position_map[pos]] = pfb;
        //cerr<<"Assigned PFB "<<pfb<<" to position "<<pos<<" with index "<<existing_position_map[pos]<<endl;
      }
    }
    ifs_pfb.close();
  }
  
  return true;
}

void HMM_CNV::run(){
  for(current_sample=0;current_sample<samples;++current_sample){
    init_state_objects(current_sample);
    allocate_matrices();
    cerr<<"\n\nBeginning HMM_CNV on sample "<<sample_annotations[current_sample]<<" with stromal contamination of "<<global_alpha<<endl;
    //compute_emission_cache_mat();
    //sample_mu_lrr_adjusted[current_sample] = 0;
    float mu_rr_offset = get_rr_offset();
    cerr<<"Adjusted RR mean for normal state is now: "<<(1. + mu_rr_offset) <<endl;
    cerr<<"state"<<"\t"<<"cn"<<"\t"<<"htzyg"<<"\t"<<"alpha"<<"\t"<<"AlphaHetTumor?\tmu_lrr\tsd_lrr\talpha_prior\n";
    for(int state=0;state<states;++state){
      state_object_vector[state].update_rr_mean(mu_rr_offset);
      state_object_vector[state].debug();
      state_mu_rr[state]+=mu_rr_offset;
    }
    compute_emission_cache_mat();
    // before baum welch training, initialize trans mat
    float last_loglike = sample_likelihood_log[current_sample] = -1.*LARGE;
    iteration = 0;
    //for(iteration=0;iteration<5;++iteration){
    bool rollback = false;
    do{
      last_loglike = sample_likelihood_log[current_sample]  ;
      for(int i=0;i<total_markers;++i){
        for(int j=0;j<states;++j){
          prev_prob_mat[i*states+j] = prob_mat[current_sample*
          hmm_matrix_size +i*states+j];
          //cerr<<" "<<trans_mat[i*states+j];
        }
        //cerr<<endl;
      }
      if (iteration==0) {
        this->forwardbackward();
      }else{
        this->baumwelch();
        this->forwardbackward();
      } 
      cerr<<"Last LL: "<<last_loglike<<" and current: "<<sample_likelihood_log[current_sample]<<endl;
      ++iteration;
      if (sample_likelihood_log[current_sample]<last_loglike){
        rollback = true;
      }
    //}
    }while(!rollback);
    cerr<<"Rolling back to previous probability matrix\n";
    for(int i=0;i<total_markers;++i){
      for(int j=0;j<states;++j){
        prob_mat[current_sample*
        hmm_matrix_size +i*states+j] = prev_prob_mat[i*states+j] ;
        //cerr<<" "<<trans_mat[i*states+j];
      }
      //cerr<<endl;
    }
    //this->forwardbackward();
    // after baum welch training, save trans mat
    for(int i=0;i<states;++i){
      for(int j=0;j<states;++j){
        //sample_trans_mat[current_sample*trans_matrix_size +i*states+j] = 
        //trans_mat[i*states+j];
      }
    }
    print_summary();
  }
}


HMM_CNV::~HMM_CNV(){
  cerr<<"Destructor\n";
}

float HMM_CNV::get_binom_sum(int n, float pop_b, float baf){
  float binom_sum = 0;
  float sd = .1;
  float mean=.5;
  float mean_increment = 0;
  if (n>1){
  	mean = 0;
  	mean_increment =1./(n-1);
  }
  for(int k=0;k<n;++k){
  	binom_sum+=math->binom_prob(n,k,pop_b) * math->normal_pdf(baf,mean,sd);
  	mean+=mean_increment;
  }
  return binom_sum;
}



float ordered_binom(int n,int k,float p){
  float prob = 1;
  for(int i=0;i<k;++i){
    prob*=p;
  }
  for(int i=0;i<n-k;++i){
    prob*=(1-p);
  }
  return prob;
}

// this needs to be called several times for each state

float HMM_CNV::get_baf_mean(int bac, int cn,float alpha){
  if((cn>=2 && bac==0) || bac==cn){
    // if tumor homo , assume normal is always homo
    return bac/cn;
  }
  int normal_bac = 1;
  int normal_cn = 2;
  float mu = ((1.-global_alpha-alpha)*bac + (global_alpha+alpha)*normal_bac)
      / ((1.-global_alpha-alpha)*cn+(global_alpha+alpha)*normal_cn);
  //cerr<<"alpha "<<alpha<<" tumor bac,cn "<<bac<<","<<cn<<" bafmu: "<<mu<<endl;
  return mu;
}


float HMM_CNV::get_baf_sd(int state, int bac){
  //float & alpha = state_alpha[state];
  float & total_alpha = state_total_alpha[state];
  // the variance of the normal tissue
  int tumorcn = state_tumorcn_vec[state];
  //cerr<<"\nFetched tumor CN "<<tumorcn<<" with het "<<state_het_ratio[state]<<" and BAC "<<bac<<endl;
  float frac = 1.*bac/tumorcn;
  //cerr<<"frac "<<frac<<endl;
  if (frac>.5) frac = 1.-frac;
  int percent = (int)(frac * 100. + .5);
  float sdnorm = sd_baf_percent[50];
  float sdtumor = sd_baf_percent[percent];
  float var = pow((total_alpha)*sdnorm,2)
   + pow((1-total_alpha)*sdtumor,2);
  //cerr<<"debug: "<<pow((1-global_alpha-alpha)*sdtumor,2)<<endl;
  //cerr<<"alpha "<<global_alpha<<","<<alpha<<" SDs "<<sdnorm<<","<<sdtumor<<"; variance: "<<var<<endl;
  return sqrt(var);
}


state_t::state_t(int id,int tumor_cn, float global_alpha,int percent, float het_ratio, bool alpha_identifiable,bool het_identifiable, bool tumor_identifiable, float * sd_rr_alpha, float * sd_rr_loh, float  sd_rr_normal){
  this->id = id;
  this->tumor_cn = tumor_cn;
  this->het_ident = het_identifiable;
  this->alpha_ident = alpha_identifiable;
  this->tumor_ident = tumor_identifiable;
  this->het_ratio = het_ratio;
  this->total_alpha = percent/100.;
  //this->total_alpha = (tumor_cn==0||tumor_cn==2) ? .01 : global_alpha+alpha*(1-global_alpha);
  float slope = (1.-HMM_CNV::INTERCEPT)/2.;
  //this->mu_lrr = log2((1-total_alpha)*(intercept+tumor_cn*slope)+ (total_alpha));
  this->mu_rr = (1-total_alpha)*(HMM_CNV::INTERCEPT+tumor_cn*slope)+ (total_alpha);
  //cerr<<"for cn "<<tumor_cn<<" mu_rr: "<<this->mu_rr<<endl;
  if (tumor_cn==2){
    if (het_ratio>0){
      // the variance of the normal tissue
      sd_rr = (sd_rr_normal);
    }else{
      sd_rr = (sd_rr_loh[tumor_cn]);
    }
  }else{
    sd_rr = sd_rr_alpha[percent];
  }
  //float var = pow(total_alpha*sd_lrr_normal,2);
  //cerr<<"id "<<id<<" first var "<<var<<" sdlrrnormal "<<sd_lrr_normal<<endl;
  //if (het_ratio>0){
    //var += pow((1-total_alpha)*sd_lrr_het[tumor_cn],2);
    //cerr<<"id "<<id<<" second  var "<<pow((1-total_alpha)*sd_lrr_het[tumor_cn],2)<<" sdlrrhet "<<sd_lrr_het[tumor_cn]<<endl;
  //}else{
    //var += pow((1-total_alpha)*sd_lrr_loh[tumor_cn],2);
  //}
  //cerr<<"id "<<id<<" final  var "<<var<<endl;
  //this->sd_lrr = sqrt(var);
}

void state_t::update_rr_mean(float offset){
  this->mu_rr += offset;
}

void state_t::debug(){
  cerr<<id<<"\t"<<tumor_cn<<"\t"<<het_ratio<<"\t"<<total_alpha<<"\t"<<alpha_ident<<het_ident<<tumor_ident<<"\t"<<mu_rr<<"\t"<<sd_rr<<"\t"<<alpha_prior;
  cerr<<endl;
}

void HMM_CNV::init_state_objects(int current_sample){
  normal_state = -1;
  homo_deletion_state = -1;
  state_object_vector.clear();
  global_alpha = global_alphas[current_sample];
  states = 0 ;
  int percent = global_alpha*100;
  // normal state
  state_object_vector.push_back(state_t(states,2,global_alpha,percent,HET_RATIO,false,true,true, sd_rr_alpha,sd_rr_loh,sd_rr_normal));
  if (normal_state==-1) normal_state = states;
  ++states;
  // copy neutral LOH
  state_object_vector.push_back(state_t(states,2,global_alpha,percent,0,false,true, false,sd_rr_alpha,sd_rr_loh,sd_rr_normal));
  ++states;
  for(vector<int>::iterator it = alpha_percents.begin();
  it!=alpha_percents.end();it++){
    int percent = *it;
    // homozygous deletion
    state_object_vector.push_back(state_t(states,0,global_alpha,percent,HET_RATIO,true,false, true,sd_rr_alpha,sd_rr_loh,sd_rr_normal));
    ++states;
    // hemizygous deletion
    state_object_vector.push_back(state_t(states,1,global_alpha,percent,HET_RATIO,true,false, true,sd_rr_alpha,sd_rr_loh,sd_rr_normal));
    ++states;
    // amplifications
    for(int tumor_cn=3;tumor_cn<=4;++tumor_cn){
      state_object_vector.push_back(state_t(states,tumor_cn,global_alpha,percent,HET_RATIO,true,false, true,sd_rr_alpha,sd_rr_loh,sd_rr_normal));
      ++states;
    }
  }
}

void HMM_CNV::normalize(int len, float * raw_probs, float * normalized){
  float normalizer = 0;
  for (int j=0;j<len;++j){
    normalizer+=raw_probs[j];
  }
  for (int j=0;j<len;++j){
    normalized[j] = raw_probs[j]/normalizer;
    if (normalized[j]<SMALL) normalized[j] = SMALL;
  }
}

void HMM_CNV::normalize_across_alphas(float * baf_emission_vec){
  for(int cn = 0;cn<TUMOR_CN_ELEMENTS;++cn){
    if (cn!=2){
      float normalizer = 0;
      for(int state = 0;state<states;++state){
        if (state_object_vector[state].tumor_cn==cn){
           normalizer+=baf_emission_vec[state];
        }
      }
      for(int state = 0;state<states;++state){
        if (state_object_vector[state].tumor_cn==cn){
           baf_emission_vec[state]/=normalizer;
           //cerr<<"new baf for state "<<state<<": "<<baf_emission_vec[state]<<endl;
        }
      }
    }else{
      // this is a uniform probability across all alpha percents
      baf_emission_vec[normal_state] = 1./alpha_percents.size();
    }
  }
}

void HMM_CNV::compute_emission_cache_mat(){
  bool debug = false;
  cerr<<"Computing and storing emission probabilities\n";
  //ofstream ofs_emission("debugging/emission_cache");
  ostringstream oss;
  oss<<output_dir<<"/emission.debug."<<current_sample;
  ofstream ofs_debug;
  if (debug) ofs_debug.open(oss.str().data());
  for (int i=0;i<total_markers;++i){
    float pfb = pfb_vec[i];
    if (verbose_output && i%1000==0) cerr<<" "<<i;
    float & rr = rr_vec[current_sample*total_markers+i];
    float & baf = baf_vec[current_sample*total_markers+i];
    //float baf_noise = .01;
    float rr_noise= .01;
    float probs[states];
    float baf_probs[states];
    float rr_probs[states];
    for (int state=0;state<states;++state){
      if (debug) ofs_debug<<"Marker:"<<i<<",";
      // get emission of the tumor heterogeneity
      float & alpha = state_total_alpha[state];
      //float & alpha_prob = state_alpha_prior[state];
      // figure out the emission probability of the LRR
      int tumorcn = state_tumorcn_vec[state];
      float & rr_prob = rr_probs[state];
      rr_prob = math->normal_pdf(rr,state_mu_rr[state],state_sd_rr[state]);
      //cerr<<"Final SD and prob for state "<<state<<": "<<state_sd_lrr[state]*sd_lrr_inflation<<","<<lrr_prob<<endl;
      rr_prob = rr_noise+(1.-rr_noise)*rr_prob;
      // figure out the emission prob of the BAF    
      float & baf_prob = baf_probs[state];
      baf_prob = 0;
      float pointmass=.5;
      if (tumorcn==0){
        if (baf==0||baf==1){
          baf_prob+=normal_cdf;
        }else{
          baf_prob+=math->normal_pdf(baf,.5,sd_baf_homo_del);
        }
      }else{  // these other cases take into account stromal contamination
        bool status_loh = (tumorcn==1 || (tumorcn==2 && state_het_ratio[state]==0));
        int effective_tumorcn = status_loh?1:tumorcn;
        // the first two cases only happen when tumor and normal are homozygote
        if (baf==0){
          baf_prob+=math->binom_prob(effective_tumorcn,0,pfb)*pointmass;
        }else if (baf==1){
          baf_prob+=math->binom_prob(effective_tumorcn,effective_tumorcn,pfb)
          * pointmass;
        }else{
          // this loop assumes the normal genotype is a heterozygote.
          for (int bac=0;bac<=effective_tumorcn;++bac){
            baf_prob+=math->binom_prob(effective_tumorcn,bac,pfb)*math->normal_pdf(baf,get_baf_mean(bac,effective_tumorcn,alpha),get_baf_sd(state,bac));
          }
          if (status_loh){
            // assume homozygote normal too
            int bac = (baf<.5)?0:1;
            baf_prob+=math->binom_prob(effective_tumorcn,bac,pfb)*math->normal_pdf(baf,bac,sd_baf_percent[0]);
          }
        }
      }
      //cerr<<"observation: "<<i<<endl;
      
      //baf_prob = baf_noise+(1.-baf_noise)*baf_prob;
      //probs[state] = baf_prob;
      //probs[state] = rr_prob * alpha_prob;
      //probs[state] = rr_prob * baf_prob * alpha_prob;
      if (debug){
        ofs_debug<<"ID:"<<state<<","<<"BAF:"<<baf<<",RR:"<<rr<<","<<"P_RR:"<<rr_prob<<","<<"P_BAF:"<<baf_prob<<endl;
      }
    }
    normalize_across_alphas(baf_probs);
    for(int state=0;state<states;++state){
      probs[state] = rr_probs[state]*baf_probs[state]*state_alpha_prior[state];
      //emission_cache_mat[current_sample*hmm_matrix_size+i*states+state] = rr_probs[state]*baf_probs[state]*state_alpha_prior[state];
    }
    normalize(states,probs,emission_cache_mat+current_sample*hmm_matrix_size+i*states);
  }
  if (verbose_output) cerr<<endl;
  //ofs_emission.close();
  if (debug) ofs_debug.close();
  ostringstream oss6;
  oss6<<output_dir<<"/emission."<<sample_annotations[current_sample];
  ofstream ofs_emission(oss6.str().data());
  for(int i=0;i<total_markers;++i){
    ofs_emission<<i<<"\t"<<"RR:"<<(rr_vec[current_sample*total_markers+i])<<"\t"<<"BAF:"<<baf_vec[current_sample*total_markers+i];
    for(int state=0;state<states;++state){
      ofs_emission<<"\t"<<state<<":"<<emission_cache_mat[current_sample*hmm_matrix_size+i*states+state];
    }
    ofs_emission<<endl; 
  }
  ofs_emission.close();
  cerr<<"Done initializing emission_cache_mat\n";
  for(int j=0;j<states;++j){
    forward_mat[current_sample*hmm_matrix_size+j] = emission_cache_mat[current_sample*hmm_matrix_size+j];
    backward_mat[current_sample*hmm_matrix_size+(total_markers-1)*states+j] = 1;
  }
}


double HMM_CNV::addMultipleLogs(double * logs, int len){
  //cerr<<"Called addMultipleLogs\n";
  double avg=0;
  for(int i=0;i<len;++i){
    avg-=logs[i];
  }
  avg/=len;
  //avg-=10;
  //cerr<<"Average scaling: "<<avg<<endl;
  double val = 0;
  for(int i=0;i<len;++i){
    val+=exp(logs[i]+avg);
  }
  double a = log(val)-avg;
  //cerr<<"Results "<<a<<endl;
  return a;
}


void HMM_CNV::print_summary(){
  ostringstream oss1;
  ostringstream oss2;
  ostringstream oss3;
  ostringstream oss4;
  ostringstream oss5;
  oss1<<output_dir<<"/summary."<<sample_annotations[current_sample];
  oss2<<output_dir<<"/posterior_alpha."<<sample_annotations[current_sample];
  oss3<<output_dir<<"/posterior_het."<<sample_annotations[current_sample];
  oss4<<output_dir<<"/posterior_tumor_cn."<<sample_annotations[current_sample];
  oss5<<output_dir<<"/transition."<<sample_annotations[current_sample];


  cerr<<"Marginalizing for sample "<<sample_annotations[current_sample]<<" across "<<total_markers<<" markers"<<endl;
  ofstream ofs_expected(oss1.str().data());
  ofstream ofs_alpha(oss2.str().data());
  ofstream ofs_het(oss3.str().data());
  ofstream ofs_tumor_cn(oss4.str().data());
  ofstream ofs_trans(oss5.str().data());
  for(int i=0;i<states;++i){
    ofs_trans<<i;
    for(int j=0;j<states;++j){
      ofs_trans<<"\t"<<j<<":"<<sample_trans_mat[current_sample*trans_matrix_size+i*states+j];
    }
    ofs_trans<<endl;
  }
  ofs_trans.close();
  float alpha_probs[100];
  float het_probs[100];
  float tumor_cn_probs[TUMOR_CN_ELEMENTS];
  ofs_expected<<"chr\tpos\tBAF\tRR\ttumor_cn\talpha\theterozyg\n";
  for(int i = 0;i<total_markers;++i){
    ofs_alpha<<i;
    ofs_het<<i;
    ofs_tumor_cn<<i;
    memset(alpha_probs,0,sizeof(alpha_probs));
    memset(het_probs,0,sizeof(het_probs));
    memset(tumor_cn_probs,0,sizeof(tumor_cn_probs));
    for(int state=0;state<states;++state){
      state_t s = state_object_vector[state];
      float prob = prob_mat[current_sample*hmm_matrix_size+i*states+state];
      //cerr<<"state "<<state<<" id "<<s.alpha_ident<<" alpha: "<<s.total_alpha<<endl;
      if(s.alpha_ident){
        int alpha_int = (int)(s.total_alpha*100);
        alpha_probs[alpha_int]+=prob;
      }
      if(s.het_ident){
        int het_int = (int)(s.het_ratio*100);
        het_probs[het_int]+=prob;
      }
      if (s.tumor_ident){
        // if normal state, scale posterior by number of fraction states to be comparable to abberation posteriors
        tumor_cn_probs[s.tumor_cn]+=(state==normal_state)?prob*alpha_percents.size():prob;
      }
    }
    float alpha_normalizer = 0;
    float het_normalizer = 0;
    for(int j=0;j<100;++j){
      if (alpha_probs[j]>0) alpha_normalizer += alpha_probs[j];
      if (het_probs[j]>0) het_normalizer += het_probs[j];
    }
    float exp_alpha = 0;
    float exp_het = 0;
    for(int j=0;j<100;++j){
      if (alpha_probs[j]>0){
         alpha_probs[j]/=alpha_normalizer;
         ofs_alpha<<"\t"<<(j/100.)<<":"<<alpha_probs[j];
         exp_alpha+=alpha_probs[j] * (j/100.);
      }
      if (het_probs[j]>0){
         het_probs[j]/=het_normalizer;
         ofs_het<<"\t"<<(j/100.)<<":"<<het_probs[j];
         exp_het+=het_probs[j] * (j/100.);
      }
    }
    float normalizer = 0;
    for(int j=0;j<TUMOR_CN_ELEMENTS;++j){
      if (tumor_cn_probs[j]>0) normalizer += tumor_cn_probs[j];
    }
    float exp_tumor_cn=0;
    for(int j=0;j<TUMOR_CN_ELEMENTS;++j){
      if (tumor_cn_probs[j]>0) {
        tumor_cn_probs[j]/=normalizer;
        ofs_tumor_cn<<"\t"<<j<<":"<<tumor_cn_probs[j];
        exp_tumor_cn+=tumor_cn_probs[j] * j;
      }
    }
    ofs_alpha<<endl;
    ofs_het<<endl;
    ofs_tumor_cn<<endl;
    ofs_expected<<chr<<"\t"<<pos_vec[i]<<"\t"<<baf_vec[current_sample*total_markers+i]<<"\t"<<(rr_vec[current_sample*total_markers+i])<<"\t"<<exp_tumor_cn<<"\t"<<exp_alpha<<"\t"<<exp_het<<endl;
  }
  ofs_alpha.close();
  ofs_het.close();
  ofs_tumor_cn.close();
  ofs_expected.close();
}

void HMM_CNV::forwardbackward(){
  cerr<<"Running forward backward...\n";
  for(int i=0;i<total_markers;++i){
    forward_rescalings[i] = 0;
    backward_rescalings[i] = 0;
  }
  //ofstream ofs_forward("debugging/forward");
  // RUN FORWARD ALGORITHM
  for(int obs=1;obs<total_markers;++obs){
    forward_rescalings[obs] = forward_rescalings[obs-1];
  //cerr<<"Launching obs"<<total_markers<<"\n";
    if (verbose_output && obs % 1000 == 0) cerr<<" "<<obs;
    float min = 1;
    for(int currentstate=0;currentstate<states;++currentstate){
      float sum = 0;
      for(int prevstate=0;prevstate<states;++prevstate){
         sum+=forward_mat[current_sample*hmm_matrix_size+(obs-1)*states+prevstate]  * sample_trans_mat[current_sample*trans_matrix_size+prevstate*states+currentstate] * emission_cache_mat[current_sample*hmm_matrix_size+obs*states+currentstate];
      }
      if (sum<min) min = sum;
      forward_mat[current_sample*hmm_matrix_size+obs*states+currentstate] = sum;
    }
    if (min<SMALL){
      for(int currentstate=0;currentstate<states;++currentstate){
        forward_mat[current_sample*hmm_matrix_size+obs*states+currentstate]*=LARGE;
      }
      //cerr<<"Rescaled forward probabilities at marker "<<obs<<endl;
      ++forward_rescalings[obs];
    }
  }
  // initialize the probabilities at the last observation as 1 since no data beyond
  // RUN BACKWARD ALGORITHM
  for(int obs=total_markers-2;obs>=0;--obs){
    backward_rescalings[obs] = backward_rescalings[obs+1];
    if (verbose_output && obs % 1000 == 0) cerr<<" "<<obs;
    float min = 1;
    for(int currentstate=0;currentstate<states;++currentstate){
      float sum = 0;
      for(int futurestate=0;futurestate<states;++futurestate){
        sum+= backward_mat[current_sample*hmm_matrix_size+(obs+1)*states+futurestate] * sample_trans_mat[current_sample*trans_matrix_size+currentstate*states+futurestate]  * emission_cache_mat[current_sample*hmm_matrix_size+(obs+1)*states+futurestate];
      }
      if (sum<min) min = sum;
      backward_mat[current_sample*hmm_matrix_size+obs*states+currentstate] = sum;
    }
    if (min<SMALL){
      for(int currentstate=0;currentstate<states;++currentstate){
        backward_mat[current_sample*hmm_matrix_size+obs*states+currentstate]*=LARGE;
      }
      //cerr<<"Rescaled backward probabilities at marker "<<obs<<endl;
      ++backward_rescalings[obs];
    }
  }
  if (verbose_output) cerr<<endl;
  if (verbose_output) cerr<<"Forward and backward rescalings: "<<forward_rescalings[total_markers-1]<<","<<backward_rescalings[0]<<endl;
  // COMPUTE LOG LIKELIHOODS
  double sum_log[states];
  double sum_log2[states];
  for(int state=0;state<states;++state){
    sum_log[state]=log(forward_mat[current_sample*hmm_matrix_size+state])+log(backward_mat[current_sample*hmm_matrix_size+state])+backward_rescalings[0]*LOG_SMALL;
    sum_log2[state]=log(forward_mat[current_sample*hmm_matrix_size+(total_markers-1)*states+state])+log(backward_mat[current_sample*hmm_matrix_size+(total_markers-1)*states+state])+forward_rescalings[total_markers-1]*LOG_SMALL;
  }
  float likelihood_log = addMultipleLogs(sum_log,states);
  float likelihood_log2 = addMultipleLogs(sum_log2,states);
  cerr<<"Iteration: "<<iteration<<": CPU likelihood_logs: "<<likelihood_log<<","<<likelihood_log2<<endl;
  if (abs(likelihood_log-likelihood_log2)>1){
    cerr<<"Mismatch in likelihoods. See debugging dir\n";
    ofstream ofs("debugging/posterior.mat");
    ofstream ofs_f("debugging/forward.mat");
    ofstream ofs_b("debugging/backward.mat");
    for(int obs=0;obs<total_markers;++obs){
      ofs<<obs;
      ofs_f<<obs;
      ofs_b<<obs;
      for(int i=0;i<states;++i){
        ofs<<"\t"<<prob_mat[current_sample*hmm_matrix_size+obs*states+i];
        ofs_f<<"\t"<<forward_mat[current_sample*hmm_matrix_size+obs*states+i];
        ofs_b<<"\t"<<backward_mat[current_sample*hmm_matrix_size+obs*states+i];
      }
      ofs<<endl;
      ofs_f<<endl;
      ofs_b<<endl;
    }
    ofs.close();
    ofs_f.close();
    ofs_b.close();
    exit(1);
  }
  sample_likelihood_log[current_sample] = likelihood_log;
  for(int obs=0;obs<total_markers;++obs){
    for(int state=0;state<states;++state){
      prob_mat[current_sample*hmm_matrix_size+obs*states+state] = exp(log(forward_mat[current_sample*hmm_matrix_size+obs*states+state])+forward_rescalings[obs]*LOG_SMALL+log(backward_mat[current_sample*hmm_matrix_size+obs*states+state])+backward_rescalings[obs]*LOG_SMALL-likelihood_log); 
    }
  }
  cerr<<"Completed computation of posterior probability matrix\n";
  bool debug2 = false;
  if (debug2){
    ofstream ofs_prob("debugging/posterior.mat2");
    ofs_prob<<"CPU Probability matrix: \n";
    for(int obs=0;obs<total_markers;++obs){
      //float rowsum=0;
      ofs_prob<<"obs:"<<obs;
      for(int state=0;state<states;++state){
        ofs_prob<<" "<<prob_mat[current_sample*hmm_matrix_size+obs*states+state];
        //rowsum+=exp(prob_mat_log[obs*states+state]);
      }
    ofs_prob<<endl;
    //cerr<<" "<<rowsum<<endl;
    }
    ofs_prob.close();
    exit(0);
  }
}

void HMM_CNV::baumwelch(){
  cerr<<"Running Baum Welch"<<endl;
  float denom_f_arr[states];
  for(int i=0;i<states;++i){
    denom_f_arr[i] = 0;
    for(int obs=0;obs<total_markers-1;++obs){
      denom_f_arr[i]+=prob_mat[current_sample*hmm_matrix_size+obs*states+i];
    }
  }
  bool debug1 = false;
  if (debug1){
    cerr<<"CPU denom:\n";
    for(int i=0;i<states;++i){
      cerr<<" "<<denom_f_arr[i];
    }
    cerr<<endl;
    //exit(0);
  }
  // new transition kernel
  float numer_f[states*states];
  for(int i=0;i<states*states;++i) numer_f[i]=0.;
  float likelihood_log=sample_likelihood_log[current_sample];
  bool debug1a = false;
  ofstream ofs_debug;
  if (debug1a) ofs_debug.open("debugging/baum.txt");
  int max_rescalings = forward_rescalings[total_markers-1] + backward_rescalings[0];
  bool defined[max_rescalings];
  float normalizer_arr[max_rescalings];
  for(int i=0;i<max_rescalings;++i) defined[i] = false;
  for(int obs=0;obs<total_markers-1;++obs){
    if (verbose_output && obs % 1000 == 0) cerr<<" "<<obs;
    float normalizer = 0;
    int index = forward_rescalings[obs]+backward_rescalings[obs];
    if (defined[index]){
      normalizer = normalizer_arr[index];
    }else{
      normalizer = normalizer_arr[index] = exp(forward_rescalings[obs]*LOG_SMALL + backward_rescalings[obs]*LOG_SMALL - likelihood_log);
      defined[index] = true;
    }
    for(int i=0;i<states;++i){
      for(int j=0;j<states;++j){
        float val1 = 0,val2 = 0;
    //    val1=exp(log(forward_mat[current_sample*hmm_matrix_size+obs*states+i])+forward_rescalings[obs]*LOG_SMALL + log(trans_mat[i*states+j] * emission_cache_mat[current_sample*hmm_matrix_size+(obs+1)*states+j]) + log(backward_mat[current_sample*hmm_matrix_size+(obs+1) * states + j]) + backward_rescalings[obs]*LOG_SMALL - likelihood_log);
        val2=forward_mat[current_sample*hmm_matrix_size+obs*states+i]*sample_trans_mat[current_sample*trans_matrix_size+i*states+j] * emission_cache_mat[current_sample*hmm_matrix_size+(obs+1)*states+j] * backward_mat[current_sample*hmm_matrix_size+(obs+1) * states + j] * normalizer;
     
        //ofs_debug<<obs<<"\t"<<i<<"\t"<<j<<"\t"<<forward_mat[current_sample*hmm_matrix_size+obs*states+i] <<"\t"<<trans_mat[i*states+j] <<"\t"<<emission_cache_mat[current_sample*hmm_matrix_size+(obs+1)*states+j] <<"\t"<< backward_mat[current_sample*hmm_matrix_size+(obs+1) * states + j]<<endl;
        if(debug1a) ofs_debug<<obs<<"\t"<<i<<"\t"<<j<<"\t"<<val1<<"\t"<<val2<<endl;
        numer_f[i*states+j]+=val2;
      }
    }
  }
  if (debug1a) ofs_debug.close();
  if (verbose_output) cerr<<endl;
  for(int i=0;i<states;++i){
    for(int j=0;j<states;++j){
      //cerr<<"numer "<<i<<","<<j<<": "<<numer_f[i*states+j]<<endl;
      sample_trans_mat[current_sample*trans_matrix_size+i*states+j]=(numer_f[i*states+j]/denom_f_arr[i]);
      if (sample_trans_mat[current_sample*trans_matrix_size+i*states+j]<SMALL) sample_trans_mat[current_sample*trans_matrix_size+i*states+j] = SMALL;
      //trans_mat[i*states+j]= HMM_SMALL + (1.-HMM_SMALL)*trans_mat[i*states+j];
    }
  }
  
  bool debug2 = false;
  if (debug2){
    cerr<<"CPU transition\n";
    for(int i=0;i<states;++i){
      float rowsum = 0;
      for(int j=0;j<states;++j){
        rowsum+=sample_trans_mat[current_sample*trans_matrix_size+i*states+j];
        cerr<<" "<<sample_trans_mat[current_sample*trans_matrix_size+i*states+j];
      }
      cerr<<" rowsum: "<<rowsum<<endl;
    }
  }
}

float HMM_CNV::get_rr_offset(){
  //return 0;
  float rr_offset = 0;
  float denom = 0;
  float epsilon = .01;
  for(int i=0;i<total_markers;++i){
    float & rr = rr_vec[current_sample*total_markers+i];
    float & baf = baf_vec[current_sample*total_markers+i];
    //float rr = pow(2,lrr);
    if (abs(baf-.5)<epsilon){
      float weight = math->normal_pdf(baf,.5,.001); 
      rr_offset+=weight * (rr-1.);
      denom+=weight; 
    }
  }
  rr_offset = denom>0?rr_offset/denom:0;
  return rr_offset;
}


